#include "Planning2D.h"
#include <string>
#include "UniformSampler.h"
#include "GridBasedSampler.h"

using namespace std;

const int NUM_SAMPLES_PER_ITERATION = 1000;

Planning2D::Planning2D(void)
	: numSamplesPerIteration_(NUM_SAMPLES_PER_ITERATION), sampler_(NULL), totalTime(0), samplingTime(0), clearTime(0), knnTime(0), linkTime(0)
{
}

Planning2D::~Planning2D(void)
{
	delete sampler_;
}

bool Planning2D::init(int argc, char *argv[])
{
	if (argc < 3)
		return false;

	string fileName = argv[1];
	radius_ = (float)atof(argv[2]);

	if (argc > 3)
	{
		float gridSize = (float)atof(argv[3]);
		sampler_ = new GridBasedSampler(gridSize);
	}
	else
		sampler_ = new UniformSampler();

	environment_.loadXml(fileName);

	int left, right, top, bottom;
	environment_.getBoundingBox(left, right, top, bottom);
	sampler_->setBounds(left, right, top, bottom, radius_);
	roadMap_.init((float)left, (float)right, (float)bottom, (float)top);

	return true;
}

void Planning2D::render()
{
	roadMap_.render();
	environment_.render();
}

void Planning2D::getBoundingBox(int& left, int& right, int& top, int& bottom) const
{
	environment_.getBoundingBox(left, right, top, bottom);
}

void Planning2D::addSamples()
{
	clock_t startTime = clock();
	clock_t startTime2;
	for (int i = 0; i < numSamplesPerIteration_; ++i)
	{
		startTime2 = clock();
		Vector2 newPoint = sampler_->sample();
		samplingTime += clock() - startTime2;

		startTime2 = clock();
		bool isClear = environment_.clear(newPoint, radius_);
		clearTime += clock() - startTime2;
		sampler_->lastSampleClearFeedback(isClear);
		if (isClear)
			roadMap_.addVertex(newPoint, environment_, radius_, knnTime, linkTime);
	}
	totalTime += clock() - startTime;
} 

void Planning2D::printTimings() const
{
	cout << "Vertices        : " << roadMap_.getNumVertices() << endl;
	cout << "Edges           : " << roadMap_.getNumEdges() << endl;
	cout << "TotalTime       : " << totalTime / (double)CLOCKS_PER_SEC << endl;
	cout << "SamplingTime    : " << samplingTime / (double)CLOCKS_PER_SEC << endl;
	cout << "CLEARTime       : " << clearTime / (double)CLOCKS_PER_SEC << endl;
	cout << "k-NNTime        : " << knnTime / (double)CLOCKS_PER_SEC << endl;
	cout << "LINKTime        : " << linkTime / (double)CLOCKS_PER_SEC << endl;
}